#include "PointMass.h"

namespace coolpool
{
	PointMass::PointMass(const PointMass & point_mass) :
		DynamicEntity(point_mass)
	{
		mass_ = point_mass.mass_;
		center_of_mass_location_ = point_mass.center_of_mass_location_;
		linear_momentum_ = point_mass.linear_momentum_;
		force_ = point_mass.force_;
		std::copy(&point_mass.status_point_mass[0], &point_mass.status_point_mass[9], &status_point_mass[0]);
	}

	PointMass::PointMass() :
		DynamicEntity()
	{
		mass_ = 1.0;
		center_of_mass_location_ = math_tools::Vector3D(0.0f, 0.0, 0.0);
		force_ = math_tools::Vector3D(0.0, 0.0, 0.0);
	}

	PointMass::~PointMass()
	{

	}

	void PointMass::clear()
	{
		force_ = math_tools::Vector3D(0.0, 0.0, 0.0);
	}

	// Updates the object position based on actual linear velocity.
	// On the end it clears the forces acting on the body, acutally this allows to go "back in time", during
	// a time step. If a negative time step is passed to the method, it correctly goes back
	// to the previous state. It is very important for the collision detection engine.
	void PointMass::onUpdate(double timeStep)
	{
		savePointMass();
		// Compute new position and speed by Euler.
		linear_momentum_ += force_;
		center_of_mass_location_ = center_of_mass_location_ + (getLinearVelocity() * timeStep);
		force_ = math_tools::Vector3D(0.0, 0.0, 0.0);
	}

	void PointMass::reverse()
	{
		center_of_mass_location_ = math_tools::Vector3D(status_point_mass[0], status_point_mass[1], status_point_mass[2]);
		linear_momentum_ = math_tools::Vector3D(status_point_mass[3], status_point_mass[4], status_point_mass[5]);
		force_ = math_tools::Vector3D(status_point_mass[6], status_point_mass[7], status_point_mass[8]);
	}
}
